import PgsContactConstraintSolver from "../solver/pgs-contact-constraint-solver";
import CONSTANT from '../../constant';

export default class ContactConstraint {
    constructor(manifold) {
        this._solver = new PgsContactConstraintSolver(this);
        this._manifold = manifold;
        this._b1=null;
        this._b2=null;
        this._s1=null;
        this._s2=null;
        this._positionCorrectionAlgorithm=null;
        this._tf1=null;
        this._tf2=null;
    }
    _getVelocitySolverInfo(timeStep,info) {
        info.b1 = this._b1;
        info.b2 = this._b2;
        let normal=this._manifold.normal,tangent=this._manifold.tangent,binormal=this._manifold.binormal;
        let normalX = normal[0],normalY = normal[1],normalZ = normal[2];
        let tangentX = tangent[0],tangentY = tangent[1],tangentZ = tangent[2];
        let binormalX = binormal[0],binormalY = binormal[1],binormalZ = binormal[2];
        let friction = Math.sqrt(this._s1._friction * this._s2._friction);
        let restitution = Math.sqrt(this._s1._restitution * this._s2._restitution);
        let num = this._manifold._numPoints;
        info.numRows = 0;
        let _g = 0;
        while(_g < num) {
            let p = this._manifold._points[_g++];
            let relPos1=p.relPos1,relPos2=p.relPos2;
            if(p._depth < 0) {
                p._disabled = true;
                let _this = p._impulse.elements;
                _this[0] = _this[1] = _this[2] = _this[3] = _this[4] = _this[5] = _this[6] = 0;
                continue;
            } else {
                p._disabled = false;
            }
            let row = info.rows[info.numRows++];
            row.friction = friction;
            row.cfm = 0;
            let j = row.jacobianN.elements;
            j[0] = normalX;j[1] = normalY;j[2] = normalZ;
            j[3] = normalX;j[4] = normalY;j[5] = normalZ;
            j[6] = relPos1[1] * normalZ - relPos1[2] * normalY;j[7] = relPos1[2] * normalX - relPos1[0] * normalZ;j[8] = relPos1[0] * normalY - relPos1[1] * normalX;
            j[9] = relPos2[1] * normalZ - relPos2[2] * normalY;j[10] = relPos2[2] * normalX - relPos2[0] * normalZ;j[11] = relPos2[0] * normalY - relPos2[1] * normalX;
            j = row.jacobianT.elements;
            j[0] = tangentX;j[1] = tangentY;j[2] = tangentZ;
            j[3] = tangentX;j[4] = tangentY;j[5] = tangentZ;
            j[6] = relPos1[1] * tangentZ - relPos1[2] * tangentY;j[7] = relPos1[2] * tangentX - relPos1[0] * tangentZ;j[8] = relPos1[0] * tangentY - relPos1[1] * tangentX;
            j[9] = relPos2[1] * tangentZ - relPos2[2] * tangentY;j[10] = relPos2[2] * tangentX - relPos2[0] * tangentZ;j[11] = relPos2[0] * tangentY - relPos2[1] * tangentX;
            j = row.jacobianB.elements;
            j[0] = binormalX;j[1] = binormalY;j[2] = binormalZ;
            j[3] = binormalX;j[4] = binormalY;j[5] = binormalZ;
            j[6] = relPos1[1] * binormalZ - relPos1[2] * binormalY;j[7] = relPos1[2] * binormalX - relPos1[0] * binormalZ;j[8] = relPos1[0] * binormalY - relPos1[1] * binormalX;
            j[9] = relPos2[1] * binormalZ - relPos2[2] * binormalY;j[10] = relPos2[2] * binormalX - relPos2[0] * binormalZ;j[11] = relPos2[0] * binormalY - relPos2[1] * binormalX;
            let b1v=this._b1.vel,b1a=this._b1.angVel,b2v=this._b2.vel,b2a=this._b2.angVel;
            j = row.jacobianN.elements;
            let rvn = j[0] * b1v[0] + j[1] * b1v[1] + j[2] * b1v[2] + (j[6] * b1a[0] + j[7] * b1a[1] + j[8] * b1a[2]) -
                (j[3] * b2v[0] + j[4] * b2v[1] + j[5] * b2v[2] + (j[9] * b2a[0] + j[10] * b2a[1] + j[11] * b2a[2]));
            if(rvn < -CONSTANT.SETTING_CONTACT_ENABLE_BOUNCE_THRESHOLD && !p._warmStarted) {
                row.rhs = -rvn * restitution;
            } else {
                row.rhs = 0;
            }
            if(this._positionCorrectionAlgorithm === CONSTANT.POSITION_CORRECTION_ALGORITHM_BAUMGARTE) {
                if(p._depth > CONSTANT.SETTING_LINEAR_SLOP) {
                    let minRhs = (p._depth - CONSTANT.SETTING_LINEAR_SLOP) * CONSTANT.SETTING_VELOCITY_BAUMGARTE * timeStep.invDt;
                    if(row.rhs < minRhs) {
                        row.rhs = minRhs;
                    }
                }
            }
            if(!p._warmStarted) {
                let _this = p._impulse.elements;
                _this[0] = _this[1] = _this[2] = _this[3] = _this[4] = _this[5] = _this[6] = 0;
            }
            row.impulse = p._impulse;
        }
    }
    _getPositionSolverInfo(info) {
        info.b1 = this._b1;
        info.b2 = this._b2;
        let normal=this._manifold.normal;
        let normalX = normal[0],normalY = normal[1],normalZ = normal[2];
        let num = this._manifold._numPoints;
        info.numRows = 0;
        let _g = 0;
        while(_g < num) {
            let p = this._manifold._points[_g++];
            let relPos1=p.relPos1,relPos2=p.relPos2;
            if(p._disabled) {
                continue;
            }
            let row = info.rows[info.numRows++];
            let j = row.jacobianN.elements;
            j[0] = normalX;j[1] = normalY;j[2] = normalZ;
            j[3] = normalX;j[4] = normalY;j[5] = normalZ;
            j[6] = relPos1[1] * normalZ - relPos1[2] * normalY;j[7] = relPos1[2] * normalX - relPos1[0] * normalZ;j[8] = relPos1[0] * normalY - relPos1[1] * normalX;
            j[9] = relPos2[1] * normalZ - relPos2[2] * normalY;j[10] = relPos2[2] * normalX - relPos2[0] * normalZ;j[11] = relPos2[0] * normalY - relPos2[1] * normalX;
            row.rhs = p._depth - CONSTANT.SETTING_LINEAR_SLOP;
            if(row.rhs < 0) {
                row.rhs = 0;
            }
            row.impulse = p._impulse;
        }
    }
    _syncManifold() {
        this._manifold._updateDepthsAndPositions(this._tf1,this._tf2);
    }
    getShape1() {
        return this._s1;
    }
    getShape2() {
        return this._s2;
    }
    getManifold() {
        return this._manifold;
    }
    isTouching() {
        let _g = 0, _g1 = this._manifold._numPoints;
        while(_g < _g1) if(this._manifold._points[_g++]._depth >= 0) {
            return true;
        }
        return false;
    }
}